#pragma once
#include <iostream>
#include <list>
#include <string>
#include <json.hpp>

#include "point.hpp"
#include "map.hpp"

namespace dw
{
    class PointAstar
    {
    private:
        Grid current;

        /* Heuristic Function Value */
        int f; // f = g + h
        int g; // Moving Cost from START to CURRENT (Known)
        int h; // Estimated Cost from CURRENT to END

        /* whether the Point is visited */
        bool visited;

        /* the point comming from */
        PointAstar *Parent;

        int moveCost(const Grid &p1, const Grid &p2);
        int estimateCost(const Grid &p1, const Grid &p2);

    public:
        // Default Constructor with no args
        // Default: Origin Point (0, 0)
        //          Zero Heuristic Function Value
        //          Never visited
        //          From nowhere
        PointAstar()
        {
            current = {0, 0};
            f = g = h = 0;
            visited = false;
            Parent = nullptr;
        }

        PointAstar(const Grid cur, PointAstar *const p = nullptr)
        {
            current = cur;
            visited = true;
            Parent = p;
            f = g = h = 0;
        }

        void calCost(const Grid &end)
        {
            if (!Parent)
            {
                g = 0;
                h = estimateCost(this->current, end);
                f = g + h;
            }
            else
            {
                g = Parent->getMoveCost() + moveCost(this->current, Parent->current);
                h = estimateCost(this->current, end);
                f = g + h;
            }
        }

        bool isVisited() const { return visited; }
        void setVisited() { visited = true; }

        int getCost() const { return f; }
        int getMoveCost() const { return g; }
        int getEstimateCost() const { return h; }

        bool operator<(const PointAstar &t)
        {
            return this->f < t.f;
        }

        bool operator>(const PointAstar &t)
        {
            return this->f > t.f;
        }

        void SetFGH(int _g, int _h)
        {
            g = _g;
            h = _h;
            f = g + h;
        }

        const Grid &coord() const
        {
            return current;
        }

        PointAstar *parent() const
        {
            return Parent;
        }
    };

    // For Priority Queue
    class FromSmallToBig
    {
    public:
        bool operator()(PointAstar *const a, PointAstar *const b)
        {
            return *a > *b;
        }
    };

    class Path
    {
    public:
    
#ifdef __linux
        using iterator = std::__cxx11::list<Coord>::iterator;
        using const_iterator = std::__cxx11::list<Coord>::const_iterator;
#elif defined(_WIN32)
        using iterator = std::list<Coord>::iterator;
        using const_iterator = std::list<Coord>::const_iterator;
#endif

    private:
        std::list<Coord> m_path;
        iterator pCurrent;

    private:
        bool checkCrossWalkable(const Map::State &mapstate, const Coord &p1, const Coord &p2);

    public:
        Path() {
            pCurrent = m_path.end();
        }

        Path(const Path& path) {
            this->m_path = path.m_path;
            this->pCurrent = this->begin();
        }

        ~Path() { /* done */ }

        // A* Algorithm
        bool operator()(const Map::State &map, const Coord &start, const Coord &end);
        bool operator()(Map::State &map, const Coord &start, const Coord &end, const int scalar, const int radius);

        // Simplify Path by Floyd Algorithm
        Path &simplify(const Map::State &mapstate);

        Path &interpolate(uint32_t num);

        Path &interpolate(double dis);

    public:
        uint32_t length() const {
            return m_path.size();
        }

        Path &push_back(const Coord &p)
        {
            m_path.push_back(p);
            return *this;
        }

        Path &push_front(const Coord &p)
        {
            m_path.push_front(p);
            return *this;
        }

        const Coord &front() const
        {
            return m_path.front();
        }

        const Coord &back() const
        {
            return m_path.back();
        }

        bool empty() const
        {
            return m_path.empty();
        }

        size_t len() const
        {
            return m_path.size();
        }

        iterator begin()
        {
            return m_path.begin();
        }

        iterator end()
        {
            return m_path.end();
        }

        const_iterator begin() const
        {
            return m_path.begin();
        }

        const_iterator end() const
        {
            return m_path.end();
        }

        Coord current() const {
            if (!empty()) {
                return *pCurrent;
            }
            return Coord({-1, -1});
        }

        Coord prev() {
            if (empty()) return Coord({-1, -1});
            if (pCurrent != m_path.begin()) --pCurrent;    
            return *pCurrent;
        }

        Coord next() {
            if (empty()) return Coord({-1, -1});
            if (pCurrent != --m_path.end()) ++pCurrent;    
            return *pCurrent;
        }

        Coord start() const {
            if (!empty()) {
                return *m_path.begin();
            } else {
                return Coord({-1, -1});
            }
        }

        Coord destination() const {
            if (!empty()) {
                return *(--m_path.end());
            }
            return Coord({-1, -1});
        }

    public:
        friend void to_json(nlohmann::json &j, const Path &path);
        friend void from_json(const nlohmann::json &j, Path &path);
        std::string encode();
        Path decode(std::string data_msg);

        bool show(const Map::State &map);

        // Output Coords
        friend std::ostream &operator<<(std::ostream &os, const Path &path);
    };

    void showPathOnMap(Map &map, const Path &path);
}